import os
import re

ROS_DIR="/opt/ros/noetic"

all_type = set()
for root,dirs,fns in os.walk(ROS_DIR):
    for fn in fns:
        if fn.endswith(".msg"):
            with open(os.path.join(root,fn),"r") as f:
                print(os.path.join(root,fn))
                ls=f.readlines()
                for l in ls:
                    l=l.strip()
                    if "#" in l:
                        l=l.split("#")[0].strip()
                    if not l:
                        continue
                    if "=" in l:
                        l=l.split("=")[0].strip()
                    while "  " in l:
                        l=l.replace("  "," ")
                    try:
                        _type,_name = l.split(" ")
                        if _type.endswith("]"):
                            _type=_type.split("[")[0]
                        if "/" in _type:
                            a,b=_type.split("/")
                            fn = a+"/msg/"+b+".msg"
                            if os.path.exists(os.path.join(ROS_DIR,"share",fn)):
                                continue
                        else:
                            fn=os.path.join(root,_type)+".msg"
                            if os.path.exists(fn):
                                continue
                            
                            fn=os.path.join(ROS_DIR,"share/std_msgs/msg",_type)+".msg"
                            if os.path.exists(fn):
                                continue
                    except:
                        print("err:[",l,"]",l.split(" "))
                        exit()
                    all_type.add(_type)
with open("msg_types.txt","w") as f:
    for t in sorted(all_type):
        f.write(t+"\n")
print("total msg type:",len(all_type))


'''
序列化格式：
类型，是否数组，数组长度，变量名，值
str  int8     int32     str  ，根据类型名
'''
def PutMessage(msg):
    if msg.type in ["int8","int16"]:#基础类型
        '''
        packagetools.Put_string(msg.type)    #类型名
        packagetools.Put_int8(msg.is_array)   #是否数组
        packagetools.Put_int32(msg.array_len)  #数组长度
        packagetools.Put_string(msg.name)      #变量名
        if msg.is_array:
            for value in msg.value:
                packagetools.Put_???(value)  #值
        else:
            packagetools.Put_???(msg.value)  #值
        '''
    else:#复合类型
        '''
        # msg.type 可能是 geometry_msgs/PoseWithCovariance 的字符串，如果不是系统基础类型，需要找到对应的.msg文件，解析他的每一行，递归调用PutMessage
        packagetools.Put_string(msg.type)    #类型名
        packagetools.Put_int8(msg.is_array)   #是否数组
        packagetools.Put_int32(msg.array_len)  #数组长度
        packagetools.Put_string(msg.name)      #变量名
        if msg.is_array:
            for value in msg.value:
                PutMessage(value)            #值
        else:
            PutMessage(msg.value)            #值
        '''
